from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    use_sim_time = LaunchConfiguration('use_sim_time')
    
    # 获取配置文件路径
    pkg_dir = get_package_share_directory('agv_startup')
    config_file = os.path.join(pkg_dir, 'config', 'slam_toolbox.yaml')
    
    # 声明参数
    declare_use_sim_time_argument = DeclareLaunchArgument(
        'use_sim_time',
        default_value='false',
        description='Use simulation/Gazebo clock'
    )
    
    # 配置SLAM节点
    slam_toolbox_node = Node(
        parameters=[
            config_file,
            {'use_sim_time': use_sim_time}
        ],
        package='slam_toolbox',
        executable='sync_slam_toolbox_node',
        name='slam_toolbox',
        output='screen'
    )
    
    # 配置TF节点
    static_tf_node = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name='static_transform_publisher',
        arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom']
    )
    bringup_dir = get_package_share_directory('agv_startup')
    launch_dir = os.path.join(bringup_dir, 'launch')

    robot = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(launch_dir, 'agv.launch.py')),
        launch_arguments={'carto_slam': 'false'}.items(),
    )
    description = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(launch_dir, 'robot_mode_description.launch.py')),
        launch_arguments={'carto_slam': 'false'}.items(),
    )
    elf = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(launch_dir, 'elf.launch.py')),
        launch_arguments={'carto_slam': 'false'}.items(),
    )
    return LaunchDescription([
        robot,
        description,
        elf,
        declare_use_sim_time_argument,
        slam_toolbox_node,
        static_tf_node
    ])